function [fit_error, pars, goodfit]=curve_fitting(x_data,y_data, ...
    algorithm,search,model,order,...
    init,PMASK,LBOUND,UBOUND,Par_1,Par_2, maxiter) %#ok<INUSL>
%   [fit_error,lam]=curve_fit(Data,algorithm,search,model,order,init,alpha)
%
%	Curve Fitter, PK 10/11/96 Fits the input data with an algorithm chosen
%	in <algorithm>
%   and a cost function according to the model chosen in <model>
%	supported algorithms:
%					'simplex':simplex 'gauss': Gauss-Newton
%					'levenberg':Levenberg-Marquardt 'minimax': Seq.
%					Quadratic Progr.
%   if no algorithm is supplied, simplex is chosen
%	for gauss and levenberg the <search> method can be chosen
%					'mixed':Mixed Polynomial Interpolation') 'cubic':Cubic
%					Interpolation')
%	the order (that is the number of parameters is chosen in <order>
%	supported models:
%			1: multiple exponetials, <order> linear and <order> nonlinear
%			terms 2: single boltzman fit, <order> should be 3 3: single
%			boltzman fit corrected for reversal pot, <order> should be 3 5:
%			double exponential fit  <order> should be 4
%  	<init> gives the initial values, if <init> is 0 initial values of 0.1
%  	are used
%
%	<alpha> gives the temperature factor (0.04 at 22 deg C)
goodfit = 1;
if(~exist('maxiter', 'var'))
    maxiter = 250;
end;
pars=init';
echo off

method=-1; %#ok<NASGU>
if     strcmp(algorithm,'simplex'), 	method=1;
elseif strcmp(algorithm,'gauss'), 		method=2;
elseif strcmp(algorithm,'levenberg'), 	method=3;
elseif strcmp(algorithm,'minimax'), 	method=4;
else
    disp('no method specified, taking simplex');
    method=1;
end

OPTIONS=0;
if method==1, 		OPTIONS(5)=1;	%simplex
elseif method==2, 	OPTIONS(5)=1; 	%gauss
end
if method ~=1 && method ~= 4
    if strcmp(search,'mixed'), 		OPTIONS(7)=0;
    elseif strcmp(search,'cubic'), 	OPTIONS(7)=1;
    end
end
OPTIONS(2)=1e-4;		%accuracy 1e-3

if(isempty(maxiter))
    OPTIONS(14)=600;
else
    OPTIONS(14)=maxiter;
end
OPTIONS = foptions(OPTIONS);
OPTIONS(14) = maxiter;

if method==2 || method==3
    [pars,OPTIONS, goodfit]=leastsqp('fit_func', pars,OPTIONS,[], ...
        PMASK,LBOUND,UBOUND, ...
        x_data,y_data,model,Par_1,Par_2);

elseif method==1
    [pars,OPTIONS]=fminsp(PMASK,LBOUND,UBOUND, ...
        'norm(fit_func(x,P1,P2,P3,P4,P5))', pars, OPTIONS, ...
    [],x_data,y_data,model,Par_1,Par_2);
else	%method 4
    OPTIONS(15)=length(x_data);
    [pars,OPTIONS]=minimaxp('f=fit_func(x,P1,P2,P3,P4,P5); g=[];', ...
        pars, OPTIONS,[],[],[],x_data,y_data,model,Par_1,Par_2);
end
fit_error=OPTIONS(8);
return

%--------------------------------------------------------------------------
%------- LEAST SQUARE   for GAUSS and LEVENBERG
function [x,OPTIONS,goodfit,f] = leastsqp(FUN,x,OPTIONS,GRADFUN,...
    PMASK,LBOUND,UBOUND, ...
    P1,P2,P3,P4,P5,P6,P7,P8,P9,P10) %#ok<INUSD>
goodfit = 1; % clear to 0 if have singular/badly scaled matrix
lastwarn = ''; % clear last warning
init = x;
XOUT=x(:);
[nvars]=length(XOUT);
% [no_sim_fits,j]=size(P1);

evalstr = FUN;
evalstr2 = '';

if ~any(FUN<48)
    evalstr=[evalstr, '(x'];
    for i=1:nargin - 7
        evalstr = [evalstr,',P',int2str(i)]; %#ok<AGROW>
    end
    evalstr = [evalstr, ')'];
end

if nargin < 3, OPTIONS=[]; end
if nargin < 4, GRADFUN=[]; end

if ~isempty(GRADFUN)
    evalstr2 = GRADFUN;
    if ~any(GRADFUN<48)
        evalstr2 = [evalstr2, '(x'];
        for i=1:nargin - 4
            evalstr2 = [evalstr2,',P',int2str(i)]; %#ok<AGROW>
        end
        evalstr2 = [evalstr2, ')'];
    end
end

f = eval(evalstr);
f=f(:);
nfun=length(f);
GRAD=zeros(length(XOUT),nfun);
OLDX=XOUT; %#ok<NASGU>
MATX=zeros(3,1);
MATL=[f'*f;0;0];
OLDF=f'*f; %#ok<NASGU>
FIRSTF=f'*f;
[OLDX,OLDF,OPTIONS]=lsintp(XOUT,f,OPTIONS);
PCNT = 0;
EstSum=0.5;
GradFactor=0;
CHG = 1e-7*abs(XOUT)+1e-7*ones(nvars,1);

OPTIONS(10)=1;
status=-1;
isfirst = 1;
while status~=1

    % Work Out Gradients
    if isempty(GRADFUN) || OPTIONS(9)
        OLDF=f;
        CHG = sign(CHG+eps).*min(max(abs(CHG),OPTIONS(16)),OPTIONS(17));
        for gcnt=1:nvars
            temp = XOUT(gcnt);
            XOUT(gcnt) = temp +CHG(gcnt);
            if(PMASK(gcnt) == 1)
                if(XOUT(gcnt) < LBOUND(gcnt))
                    XOUT(gcnt) = LBOUND(gcnt);
                end;
                if(XOUT(gcnt) > UBOUND(gcnt))
                    XOUT(gcnt) = UBOUND(gcnt);
                end;
            else
                XOUT(gcnt) = init(gcnt);
            end;


        x(:) = XOUT;
            f(:) = eval(evalstr);
            GRAD(gcnt,:)=(f-OLDF)'/(CHG(gcnt));
            %             if(all(isnan(GRAD)) & isfirst)
            %                 fprintf(1, '*** Nan Grad at var %d, CHG = %d, OPT16: %f
            %                 opt17: %f\n', gcnt, CHG(gcnt),OPTIONS(16),OPTIONS(17) );
                    %                 f isfirst = 0;
                    %             end
            XOUT(gcnt) = temp;
        end
        f = OLDF;
        OPTIONS(10)=OPTIONS(10)+nvars;
        % Gradient check
        if OPTIONS(9) == 1
            GRADFD = GRAD;
            x(:)=XOUT; GRAD = eval(evalstr2);
     XOUT = clipxout(XOUT, PMASK, LBOUND, UBOUND, init);
            graderr(GRADFD, GRAD, evalstr2);
            OPTIONS(9) = 0;
        end
    else
      XOUT = clipxout(XOUT, PMASK, LBOUND, UBOUND, init);

        x(:) = XOUT;
        OPTIONS(11)=OPTIONS(11)+1;
        GRAD = eval(evalstr2);
    end
    %     if(all(isnan(GRAD))) fprintf(1, '****** GRAD IS EMPTY, iteration
    %     %d\n', OPTIONS(10));
%     
%     end
    % Try to set difference to 1e-8 for next iteration
    if nfun==1
        CHG = nfun*1e-8./GRAD;
    else
        CHG = nfun*1e-8./sum(abs(GRAD)')';
    end

    gradf = 2*GRAD*f;
    fnew = f'*f;
    %---------------Initialization of Search Direction------------------
    if status==-1
        if cond(GRAD)>1e8
            SD=-(GRAD*GRAD'+(norm(GRAD)+1)*(eye(nvars,nvars)))\(GRAD*f);
            if OPTIONS(5)==0, GradFactor=norm(GRAD)+1; end
            how='COND';
        else
            	%	SD=GRAD'\(GRAD'*X-F)-X;
            SD=-(GRAD*GRAD'+GradFactor*(eye(nvars,nvars)))\(GRAD*f);
        end
        FIRSTF=fnew;
        OLDG=GRAD; %#ok<NASGU>
        GDOLD=gradf'*SD;
        % OPTIONS(18) controls the initial starting step-size. If
        % OPTIONS(18) has been set externally then it will be non-zero,
        % otherwise set to 1.
        if OPTIONS(18) == 0, OPTIONS(18)=1; end
        if OPTIONS(1)>0
            disp([sprintf('%5.0f %12.6g %12.3g ',OPTIONS(10),fnew, ...
                OPTIONS(18)),sprintf('%12.3g  ',GDOLD)]);
        end
        XOUT=XOUT+OPTIONS(18)*SD;
     XOUT = clipxout(XOUT, PMASK, LBOUND, UBOUND, init);
     if OPTIONS(5)==0
            newf=GRAD'*SD+f;
            GradFactor=newf'*newf;
            SD=-(GRAD*GRAD'+GradFactor*(eye(nvars,nvars)))\(GRAD*f);
        end
        newf=GRAD'*SD+f;
        XOUT=XOUT+OPTIONS(18)*SD;
      XOUT = clipxout(XOUT, PMASK, LBOUND, UBOUND, init);
       EstSum=newf'*newf;
        status=0;
        if OPTIONS(7)==0; PCNT=1; end

    else
        %-------------Direction Update------------------
        gdnew=gradf'*SD;
        if OPTIONS(1)>0,
            num=[sprintf('%5.0f %12.6g %12.3g ',OPTIONS(10),fnew, ...
                OPTIONS(18)),sprintf('%12.3g  ',gdnew)];
        end
        if gdnew>0 && fnew>FIRSTF

            % Case 1: New function is bigger than last and gradient w.r.t.
            % SD -ve ... interpolate.
            how='inter';
            [stepsize]=cubici1p(fnew,FIRSTF,gdnew,GDOLD,OPTIONS(18));
            OPTIONS(18)=0.9*stepsize;
        elseif fnew<FIRSTF

            %  New function less than old fun. and OK for updating
            %         .... update and calculate new direction.
            [newstep,fbest] =cubici3p(fnew,FIRSTF,gdnew,GDOLD,OPTIONS(18));
            if fbest>fnew,fbest=0.9*fnew; end
            if gdnew<0
                how='incstep';
                if newstep<OPTIONS(18)
                    newstep=(2*OPTIONS(18)+1e-4);
                    how=[how,'IF']; %#ok<AGROW>
                end
                OPTIONS(18)=abs(newstep);
            else
                if OPTIONS(18)>0.9
                    how='int_step';
                    OPTIONS(18)=min([1,abs(newstep)]);
                end
            end
            % SET DIRECTION. Gauss-Newton Method
            temp=1;
            if OPTIONS(5)==1
                if OPTIONS(18)>1e-8 && cond(GRAD)<1e8
                    SD=GRAD'\(GRAD'*XOUT-f)-XOUT;
                    if SD'*gradf>eps
                        how = 'ERROR- GN not descent direction';
                    end
                    temp=0;
                else
                    if OPTIONS(1) > 0
                        fprintf(1, 'Cond, Gradient Poor - Using LM method\n')
                    end
                    how='CHG2LM';
                    OPTIONS(5)=0;
                    OPTIONS(18)=abs(OPTIONS(18));
                end
            end

            if (temp)
                % Levenberg_marquardt Method N.B. EstSum is the estimated
                % sum of squares.
                %                                 GradFactor is the value
                %                                 of lambda.
                % Estimated Residual:
                if EstSum>fbest
                    GradFactor=GradFactor/(1+OPTIONS(18));
                else
                    GradFactor=GradFactor+(fbest-EstSum)/(OPTIONS(18)+eps);
                end
                if(all(isnan(GRAD)))
                    goodfit = 0;
                end;
                SD=-(GRAD*GRAD'+GradFactor*(eye(nvars,nvars)))\(GRAD*f);

                OPTIONS(18)=1;
                estf=GRAD'*SD+f;
                EstSum=estf'*estf;
                if OPTIONS(1)>0
                    num=[num,sprintf('%12.6g ',GradFactor)]; %#ok<AGROW>
                end
            end
            gdnew=gradf'*SD;
     XOUT = clipxout(XOUT, PMASK, LBOUND, UBOUND, init);

            OLDX=XOUT;
            % Save Variables
            FIRSTF=fnew;
            OLDG=gradf; %#ok<NASGU>
            GDOLD=gdnew;

            % If quadratic interpolation set PCNT
            if OPTIONS(7)==0, PCNT=1; MATX=zeros(3,1);  MATL(1)=fnew; end
        else
            % Halve Step-length
            how='Red_Step';
            if fnew==FIRSTF,
                if OPTIONS(1)>0,
                    disp('No improvement in search direction: Terminating')
                end
                status=1;
            else
                OPTIONS(18)=OPTIONS(18)/8;
                if OPTIONS(18)<1e-8
                    OPTIONS(18)=-OPTIONS(18);
                end
            end
        end
        XOUT=OLDX+OPTIONS(18)*SD;
     XOUT = clipxout(XOUT, PMASK, LBOUND, UBOUND, init);
        if OPTIONS(1)>0,disp([num,how]),end

    end %----------End of Direction Update-------------------
    if OPTIONS(7)==0, PCNT=1; MATX=zeros(3,1);  MATL(1)=fnew; end
    % Check Termination
    if max(abs(SD))< OPTIONS(2) && (gradf'*SD) < OPTIONS(3) && ...
            max(abs(gradf)) < 10*(OPTIONS(3)+OPTIONS(2))
        if OPTIONS(1) > 0
            disp('Optimization Terminated Successfully')
        end
        status=1;
    elseif OPTIONS(10)>OPTIONS(14)
        disp(sprintf('   > L-M maximum number of iterations (%i) exceeded', ...
            OPTIONS(10)));
        how='maxend';
        if OPTIONS(1)>0
            disp('Increase OPTIONS(14)')
        end
        status=1;
    else

        % Line search using mixed polynomial interpolation and
        % extrapolation.
        if PCNT~=0
            while PCNT > 0
                x(:) = XOUT; f(:)  = eval(evalstr); 
       XOUT = clipxout(XOUT, PMASK, LBOUND, UBOUND, init);
              OPTIONS(10)=OPTIONS(10)+1;
                fnew = f'*f;
                if fnew <= OLDF'*OLDF, OX = XOUT; OLDF=f; end
                how='p1';%PK
                [PCNT,MATL,MATX,steplen,fnew,how]=searchqp(PCNT,fnew, ...
                    OLDX,MATL,MATX,SD,GDOLD,OPTIONS(18),how);
                OPTIONS(18)=steplen;
                XOUT=OLDX+steplen*SD;
        XOUT = clipxout(XOUT, PMASK, LBOUND, UBOUND, init);
             if fnew==FIRSTF,  PCNT=0; end
            end
            XOUT = OX;
      XOUT = clipxout(XOUT, PMASK, LBOUND, UBOUND, init);
           f=OLDF;
        else
            x(:)=XOUT; f(:) = eval(evalstr); OPTIONS(10)=OPTIONS(10)+1;
 
        end
    end
end

if(~isempty(lastwarn))
    fprintf(1, '%s\n', lastwarn);
    goodfit = 0; % change flag
end;
OPTIONS(8) = fnew;
XOUT=OLDX;
     XOUT = clipxout(XOUT, PMASK, LBOUND, UBOUND, init);
x(:)=XOUT;
return

function [XOUT] = clipxout(XOUT, PMASK, LBOUND, UBOUND, init)
for i = 1:length(XOUT)
    if(PMASK(i) == 1)
        if(XOUT(i) < LBOUND(i))
            XOUT(i) = LBOUND(i);
        end;
        if(XOUT(i) > UBOUND(i))
            XOUT(i) = UBOUND(i);
        end;
 
    else
        XOUT(i) = init(i);
    end;
end;
return
%--------------------------------------------------------------------------
%------- FMINS for SIMPLEX
function [x, options] = fminsp(PMASK,LBOUND,UBOUND,funfcn,x,options, ...
    grad, varargin) %#ok<INUSL>
%FMINS  Minimize function of several variables.  ==> SIMPLEX
%   X = FMINS('F',X0) attempts to return a vector X which is a local
%   minimizer of F(x) near the starting vector X0.  'F' is a string
%   containing the name of the objective function to be minimized. F(x)
%   should be a scalar valued function of a vector variable.
%
%   X = FMINS('F',X0,OPTIONS) uses a vector of control parameters. If
%   OPTIONS(1) is positive, intermediate steps in the solution are
%   displayed; the default is OPTIONS(1) = 0.  OPTIONS(2) is the
%   termination tolerance for x; the default is 1.e-4.  OPTIONS(3) is the
%   termination tolerance for F(x); the default is 1.e-4. OPTIONS(14) is
%   the maximum number of function evaluations; the default is OPTIONS(14)
%   = 200*length(x).  The other components of OPTIONS are not used as input
%   control parameters by FMIN. For more information, see FOPTIONS.
%
%   X = FMINS('F',X0,OPTIONS,[],P1,P2,...) provides for additional
%   arguments which are passed to the objective function, F(X,P1,P2,...)
%   Pass an empty matrix for OPTIONS to use the default value.
%
%   [X,OPTIONS] = FMINS(...) returns the number of function evaluations in
%   OPTIONS(10).
%
%   FMINS uses a Nelder-Mead type simplex search method.
%
%   See also FMIN, FOPTIONS.

%   Reference: J. E. Dennis, Jr. and D. J. Woods, New Computing
%   Environments: Microcomputers in Large-Scale Computing, edited by A.
%   Wouk, SIAM, 1987, pp. 116-122.

%   Copyright (c) 1984-96 by The MathWorks, Inc. $Revision: 5.11 $  $Date:
%   1996/10/28 22:13:21 $


BOUNDS=1;	%OFF=0, ON=1   for PK boundaries
if BOUNDS == 1
    %   disp('PK bounds are on')
else
    %   disp('PK bounds are off')
end
if nargin<3, options = []; end
options = foptions(options);
prnt = options(1);
tol = options(2);
tol2 = options(3);

%PMASK LBOUND UBOUND

% The input argument grad is there for compatability with FMINU in the
% Optimization Toolbox, but is not used by this function.

% Convert to inline function as needed.
funfcn = fcnchk(funfcn,length(varargin));

n = numel(x);
if (~options(14))
    options(14) = 400*n; %200 *n
end
%disp (sprintf(' n is::: %d', n))
% Set up a simplex near the initial guess.
xin = x(:); % Force xin to be a column vector
v = xin;    % Place input guess in the simplex! (credit L.Pfeffer at Stanford)
x(:) = v; fv = feval(funfcn,x,varargin{:});

% Following improvement suggested by L.Pfeffer at Stanford
usual_delta = 0.05;             % 5 percent deltas for non-zero terms
zero_term_delta = 0.00025;      % Even smaller delta for zero elements of x
% zero_term_delta = usual_delta;
for j = 1:n
    y = xin;
    if y(j) ~= 0
        %added PK to fix vars
        if PMASK(j) ==1
            y(j) = (1 + usual_delta)*y(j);
            if BOUNDS ==1
                if y(j) < LBOUND(j)
                    y(j) =LBOUND(j);
                end
                if y(j) > UBOUND(j)
                    y(j) =UBOUND(j);
                end
            end
        else
            y(j) = y(j);
        end
        % end PK
    else
        y(j) = zero_term_delta;
    end
    v = [v y]; %#ok<AGROW>
    x(:) = y; f = feval(funfcn,x,varargin{:});
    fv = [fv  f]; %#ok<AGROW>
end
[fv,j] = sort(fv);
v = v(:,j);


func_evals = n+1;
if prnt > 0
    clc
    format compact
    format short e
    home
    func_evals %#ok<NOPRT>
    disp('initial ')
    disp(' ')
    v %#ok<NOPRT>
    f %#ok<NOPRT>
end

alpha = 1;  beta = 1/2;  gamma = 2;
[n,np1] = size(v); %#ok<NASGU>
onesn = ones(1,n);
ot = 2:n+1;
on = 1:n;

% Iterate until the diameter of the simplex is less than tol.
while func_evals < options(14)
    %   if(~mod(func_evals, 100)) disp(sprintf('iteration: %d of %d',
    %   func_evals, options(14)))
    %  end

    if max(max(abs(v(:,ot)-v(:,onesn)))) <= tol && ...
            max(abs(fv(1)-fv(ot))) <= tol2
        break
    end

    % One step of the Nelder-Mead simplex algorithm

    vbar = (sum(v(:,on)')/n)';
    vr = (1 + alpha)*vbar - alpha*v(:,n+1);
    x(:) = vr;
    % beg PK
    for j=1:n
        if PMASK(j) ==0
            x(j) = y(j);
        else
            if BOUNDS ==1
                if x(j) < LBOUND(j)
                    x(j)= LBOUND(j);
                end
                if x(j) > UBOUND(j)
                    x(j) =UBOUND(j);
                end
            end
        end
    end
    % end PK

    fr = feval(funfcn,x,varargin{:});
    func_evals = func_evals + 1;
    vk = vr;  fk = fr; how = 'reflect ';
    if fr < fv(n)
        if fr < fv(1)
            ve = gamma*vr + (1-gamma)*vbar;
            x(:) = ve;
            fe = feval(funfcn,x,varargin{:});
            func_evals = func_evals + 1;
            if fe < fv(1)
                vk = ve; fk = fe;
                how = 'expand  ';
            end
        end
    else
        vt = v(:,n+1); ft = fv(n+1);
        if fr < ft
            vt = vr; ft = fr; %#ok<NASGU>
        end
        vc = beta*vt + (1-beta)*vbar;
        x(:) = vc;
        % beg PK
        for j=1:n
            if PMASK(j) ==0
                x(j) = y(j);
            else
                if BOUNDS ==1
                    if x(j) < LBOUND(j)
                        x(j) =LBOUND(j);
                    end
                    if x(j) > UBOUND(j)
                        x(j) =UBOUND(j);
                    end
                end
            end
        end
        % end PK
        fc = feval(funfcn,x,varargin{:});
        func_evals = func_evals + 1;
        if fc < fv(n)
            vk = vc; fk = fc;
            how = 'contract';
        else
            for j = 2:n
                v(:,j) = (v(:,1) + v(:,j))/2;
                x(:) = v(:,j);
                fv(j) = feval(funfcn,x,varargin{:});
            end
            func_evals = func_evals + n-1;
            vk = (v(:,1) + v(:,n+1))/2;
            x(:) = vk;
            % beg PK
            for j=1:n
                if PMASK(j) ==0
                    x(j) = y(j);
                else
                    if BOUNDS ==1
                        if x(j) < LBOUND(j)
                            x(j) =LBOUND(j);
                        end
                        if x(j) > UBOUND(j)
                            x(j) =UBOUND(j);
                        end
                    end
                end
            end
            % end PK
            fk = feval(funfcn,x,varargin{:});
            func_evals = func_evals + 1;
            how = 'shrink  ';
        end
    end
    v(:,n+1) = vk;
    fv(n+1) = fk;
    [fv,j] = sort(fv);
    v = v(:,j);

    if prnt > 0
        func_evals
        disp(how)
        disp(' ')
        v
        fv
    end
end
x(:) = v(:,1);
% beg PK
for j=1:n
    if PMASK(j) ==0
        x(j) = y(j);
    else
        if BOUNDS ==1
            if x(j) < LBOUND(j)
                x(j)=LBOUND(j);
            end
            if x(j) > UBOUND(j)
                x(j) =UBOUND(j);
            end
        end
    end
end
% end PK
if prnt > 0, format, end
options(10)=func_evals;
options(8)=min(fv);
if func_evals==options(14)
    if options(1) >= 0
        disp(['Warning: Maximum number of function evaluations (', ...
            int2str(options(14)),') has been exceeded']);
        disp( '         (increase OPTIONS(14)).')
    end
end

return

%--------------------------------------------------------------------------
%------- MINIMAX for MINIMAX
function [x,OPTIONS]=minimaxp(FUN,x,OPTIONS,VLB,VUB, ...
    GRADFUN,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10)

if nargin < 3, OPTIONS=[]; end
if nargin < 4, VLB=[]; end
if nargin < 5, VUB=[]; end
if nargin < 6, GRADFUN=[]; end
lenopt = length(OPTIONS);
if ~lenopt, OPTIONS=0; end

xnew=x(:);
OPTIONS=foptions(OPTIONS);
OPTIONS(7) = ~OPTIONS(7);
neqcstr=OPTIONS(15);

if ~any(FUN < 48)
    evalstr1 = ['[f,g]=',FUN,];
    evalstr1=[evalstr1, '(x'];
    for i=1:nargin - 6
        evalstr1 = [evalstr1,',P',int2str(i)];
    end
    evalstr1 = [evalstr1, ');'];
else
    evalstr1=[FUN,';'];
end

eval(evalstr1)
ncstr=length(f);
WEIGHT = ones(ncstr,1);
GOAL = zeros(ncstr,1);

evalstr2='';

if ~isempty(GRADFUN)
    if ~any(GRADFUN<48) % Check alphanumeric
        evalstr2 = ['[gf,gg]=',GRADFUN,'(x'];
        for i=1:nargin - 6
            evalstr2 = [evalstr2,',P',int2str(i)];
        end
        evalstr2 = [evalstr2, ');'];
        gfun  = 'goalgra';
    else
        evalstr2=[GRADFUN,';'];
    end
else
    gfun = [];
end

evalstr = [ ...
   '[xnew, OPTIONS] = constr(''goalfun'',[xnew;0],OPTIONS,VLB,VUB,gfun,neqcstr,evalstr1,evalstr2,WEIGHT,GOAL,x'];

for i=1:nargin - 6
    evalstr = [evalstr,',P',int2str(i)];
end
evalstr = [evalstr, ');'];

eval(evalstr)
OPTIONS(7) = ~OPTIONS(7);
x(:) = xnew(1:length(xnew)-1);

return


%--------------------------------------------------------------------------
%-------
function [xold,fold,para,how]=lsintp(xnew,fnew,para)

xold=xnew;
fold=fnew;
%para=foptions(para);
if para(14)==0, para(14)=length(xnew)*100;end
if para(1)>0,
    disp('')
    if para(5)>0
        disp('f-COUNT      RESID    STEP-SIZE      GRAD/SD  LINE-SEARCH')
    else
        disp('f-COUNT      RESID    STEP-SIZE      GRAD/SD        LAMBDA LINE-SEARCH')
    end
end

return



%--------------------------------------------------------------------------
%-------
function [maximum,err]=cubicp(pts,checkpt,location)

d=pts(1);
abc=[0.5 -0.5 1/6 ; -2.5 2 -0.5; 3 -1.5 1/3]*[pts(2:4)-d*ones(3,1)];
root=real(sqrt(4*(abc(2)^2)-12*abc(1)*abc(3)));
x1=(-2*abc(2)+root)/(6*abc(1));
if 6*abc(1)*x1+2*abc(2)<0
    stepmin=x1;
else
    stepmin=(-2*abc(2)-root)/(6*abc(1));
end
maximum=abc(1)*stepmin^3+abc(2)*stepmin^2+abc(3)*stepmin+d;
if nargin>1
    if location==0
        checkpt2=-abc(1)+abc(2)-abc(3)+d;
    else
        checkpt2=64*abc(1)+16*abc(2)+4*abc(3)+d;
    end
    err=abs(checkpt-checkpt2);
end

return


%--------------------------------------------------------------------------
%-------
function r=cubici1p(fnew,fold,graddnew,graddold,stepsize)

if fnew==Inf, fnew=1/eps; end
z=3*(fold-fnew)/stepsize+graddold+graddnew;
w=real(sqrt(z*z-graddold*graddnew));
r=stepsize*((z+w-graddold)/(graddnew-graddold+2*w));
return

%--------------------------------------------------------------------------
%-------
function [stepmin]=cubici2p(graddold,matl,matx)
abd=[1/3*matx.^3, 0.5*matx.^2, ones(3,1)]\(matl-graddold*matx);
root=real(sqrt(abd(2)^2-4*abd(1)*graddold));
x1=(-abd(2)+root)/(2*abd(1));
if 2*abd(1)*x1+abd(2)>0
    stepmin=x1;
else
    stepmin=(-abd(2)-root)/(2*abd(1));
end
if stepmin<0||isnan(stepmin)||stepmin==Inf
    stepmin=abs(quadip(matx,matl));
end
if isnan(stepmin),stepmin=matx(2)/2; end

% fbest=1/3*abd(1)*stepmin^3+0.5*abd(2)*stepmin^2+graddold*stepmin+matl(1);
return

%--------------------------------------------------------------------------
%-------
function [stepmin]=quadip(steps,matf)
%QUADINTER Quadraticly interpolates three points to estimate minimum.
%         This function uses QUADRATIC interpolation and the values of
%         three unevenly spaced points in order estimate the minimum of a a
%         function along a line.
%    syntax: [stepmin]=quadinter(steps,matf) where steps is a matrix
%    constraining the spacing of the points.
%          matf is a vector which contains the corresponding elements of f
%          stepmin is the step to the minimum.

%	Copyright (c) 1990 by the MathWorks, Inc. Andy Grace 7-9-90.
steps=steps(:);
% Solve simultaneous equations:
amat=[0.5*steps.*steps, steps, ones(3,1)];
abc=amat\matf(:);
stepmin=-abc(2)/abc(1);
return

%--------------------------------------------------------------------------
%-------
function [stepmin,fbest]=cubici3p(fnew,fold,graddnew,graddold,stepsize)

if fnew==Inf, fnew=1/eps; end
amat=[1/3*stepsize^3 , 0.5*stepsize^2; stepsize^2     stepsize];
bmat=[fnew-graddold*stepsize-fold; graddnew-graddold];
abd=amat\bmat;
root=real(sqrt(abd(2)^2-4*abd(1)*graddold));
x1=(-abd(2)+root)/(2*abd(1));
if 2*abd(1)*x1+abd(2)>0
    stepmin=x1;
else
    stepmin=(-abd(2)-root)/(2*abd(1));
end
if stepmin<0,  stepmin=-stepmin; end
fbest=1/3*abd(1)*stepmin^3+0.5*abd(2)*stepmin^2+graddold*stepmin+fold;
return


%--------------------------------------------------------------------------
%-------
function [stepmin]=quadinter(steps,matf)
steps=steps(:);
% Solve simultaneous equations:
amat=[0.5*steps.*steps, steps, ones(3,1)];
abc=amat\matf(:);
stepmin=-abc(2)/abc(1);
return

%--------------------------------------------------------------------------
%-------
function [pcnt, matl,matx,stepsize,fnew,how]=searchqp(pcnt,fnew, ...
    oldx,matl,matx,sd,gdold,stepsize,how)
%SEARCHQ Line search routine for FMINU and LEASTSQ functions.
%	Performs line search procedure for unconstrained and least squares
% 	optimization. Uses Quadratic Interpolation.
%	When finished pcnt returns 0.

%	Copyright (c) 1990 by the MathWorks, Inc. Andy Grace 7-9-90.
if pcnt==1
    % Case 1: Next point less than initial point.
    %	  Increase step-length based on last gradient evaluation
    if fnew<matl(1)
        % Quadratic Extrapolation using gradient of first point and values
        % of two other points.
        matl(2)=fnew;
        matx(2)=stepsize;
        newstep=-0.5*gdold*stepsize*stepsize/ ...
            (fnew-gdold*stepsize-matl(1)+eps);
        if newstep<stepsize,how=[how,'QEF ']; newstep=1.2*stepsize; end
        stepsize=1.2*newstep;
        pcnt=2;
    else
        % Case 2: New point greater than initial point. Decrease
        % step-length.
        matl(3)=fnew;
        matx(3)=stepsize;
        %Interpolate to get stepsize
        stepsize=max([1e-8*stepsize,-gdold*0.5*stepsize^2/ ...
            (fnew-gdold*stepsize-matl(1)+eps)]);
        how=[how,'R'];% original r
        pcnt=3;
    end
    % Case 3: Last run was Case 1 (pcnt=2) and new point less than
    %	  both of other 2. Replace.
elseif pcnt==2  && fnew< matl(2)
    newstep=cubici2p(gdold,[matl(1);matl(2);fnew], ...
        [matx(1);matx(2);stepsize]);
    if newstep<stepsize,how=[how, 'CEF ']; end
    matl(1)=matl(2);
    matx(1)=matx(2);
    matl(2)=fnew;
    matx(2)=stepsize;
    stepsize=min([newstep,1])+1.5*stepsize;
    stepsize=max([1.2*newstep,1.2*stepsize]);
    how=[how,'i'];
    % Case 4: Last run was Case 2: (pcnt=3) and new function still
    %	  greater than initial value.
elseif pcnt==3 && fnew>=matl(1)
    matl(2)=fnew;
    matx(2)=stepsize;
    if stepsize<1e-6
        newstep=-stepsize/2;
        %		if abs(newstep)<eps, newstep=rand(1)-0.5; how=[how,'RAND'];
        %		end
    else
        newstep=cubici2p(gdold,[matl(1);matl(3);fnew], ...
            [matx(1);matx(3);stepsize]);
    end
    matx(3)=stepsize;
    if isnan(newstep), stepsize=stepsize/2; else stepsize=newstep; end
    matl(3)=fnew;
    how=[how,'R'];
    % Otherwise must have Bracketed Minimum so do quadratic interpolation.
    %  ... having just increased step.
elseif pcnt==2 && fnew>matl(2)
    matx(3)=stepsize;
    matl(3)=fnew;
    [stepsize]=cubici2p(gdold,matl,matx);
    pcnt=4;
    % ...  having just reduced step.
elseif  pcnt==3  && fnew<matl(1)
    matx(2)=stepsize;
    matl(2)=fnew;
    [stepsize]=cubici2p(gdold,matl,matx);
    pcnt=4;
    % Have just interpolated - Check to see whether function is any better
    % - if not replace.
elseif pcnt==4
    pcnt=0;
    stepsize=abs(stepsize);
    % If interpolation failed use old point.
    if fnew>matl(2),
        fnew=matl(2);
        how='F';%original f
        stepsize=matx(2);
    end
end %if pcnt==1

return
function OPTIONS = foptions(parain)
% taken from the R11 toolbox funfun...
if(nargin < 1)
    parin = [];
end;
sizep = length(parain);
OPTIONS = zeros(1,18);
OPTIONS(1:sizep) = parain(1:sizep);
default_options = [0,1e-4,1e-4,1e-6,0,0,0,0,0,0,0,0,0,0,0,1e-7,0.1,0];
OPTIONS = OPTIONS+(OPTIONS==0).*default_options;

